#include "constants.h"
#include "CostMarker.h"
#include "library.h"
#include "Interpolate.h"
#include "ControlClient.h"
#include "LNP.h"
#include "Astar.h"
#include "gradientPlanner.h"
#include "fnavfn/Stalk.h"

class Planner{
	ros::Subscriber subg;
	ros::NodeHandle n;
	ros::ServiceServer plan_service, service;
	ros::ServiceClient sc;
	tf::TransformListener listener;
	tf::StampedTransform transform;
	ros::Publisher cost_marker_pub;

	LNP obsc;
	Astar astar;
	Interpolate intpltr;
	GradientPlanner gplanner;

	MATRIX grid, gradient, fine;
	VECTOR pathx, pathy;
	std::vector<PII> path;
	int goal_num;
	double goalx, goaly, resolution, zoom;
	double botx, boty, bott;
	int width, height;

	ros::Time lastModifiedTime;

	/*** Defined in the order in which the methods are called ***/
public:
	Planner();

	bool Stalk(fnavfn::Stalk::Request  &req, fnavfn::Stalk::Response &res);
	void getBaseFootprint();
	void queryMapServerForMap();

	//void Planner::SetStart(const geometry_msgs::PoseStamped &msg)
	//bool get_path(planner::Path::Request &req, planner::Path::Response &res );
	//void calculateGradients(MATRIX navfn);

};

